import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
import numpy as np
from enum import Enum

class State(Enum):
    GO_STRAIGHT = 0
    PREPARE     = 1
    TURN_IN     = 2
    GO_SIDE     = 3
    TURN_BACK   = 4

class Side(Enum):
    LEFT  = 1
    RIGHT = -1
    NONE  = 0

class ObstacleAvoider(Node):
    def __init__(self):
        super().__init__('obstacle_avoider')

        # 参数
        self.declare_parameter('speed', 1.2)           # Ackermann 线速度
        self.declare_parameter('steer_angle_max', 1.0) # 最大转角（弧度）
        self.declare_parameter('detection_distance', 4.0)
        self.declare_parameter('side_offset', 1.5)
        self.declare_parameter('center_tolerance', 0.15)
        self.declare_parameter('wheelbase', 1.12)       # Ackermann 小车轴距

        self.v = float(self.get_parameter("speed").value)
        self.steer_max = float(self.get_parameter("steer_angle_max").value)
        self.d_detect = float(self.get_parameter("detection_distance").value)
        self.y_side = float(self.get_parameter("side_offset").value)
        self.y_tol = float(self.get_parameter("center_tolerance").value)
        self.L = float(self.get_parameter("wheelbase").value)

        self.state = State.GO_STRAIGHT
        self.target_side = Side.NONE

        self.current_y = 0.0
        self.yaw = 0.0
        self.scan = None

        # ROS2 接口
        self.create_subscription(LaserScan, '/scan', self.scan_callback, 10)
        self.create_subscription(Odometry, '/odom', self.odom_callback, 10)
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        self.timer = self.create_timer(0.1, self.control_loop)

    def odom_callback(self, msg):
        self.current_y = msg.pose.pose.position.y
        # 获取 yaw
        q = msg.pose.pose.orientation
        siny = 2 * (q.w*q.z + q.x*q.y)
        cosy = 1 - 2 * (q.y*q.y + q.z*q.z)
        self.yaw = np.arctan2(siny, cosy)

    def scan_callback(self, msg):
        self.scan = msg

    def get_min_front(self):
        """±30° 前方最小距离"""
        if not self.scan: return float('inf')
        angles = np.arange(self.scan.angle_min,
                           self.scan.angle_max,
                           self.scan.angle_increment)
        ranges = np.array(self.scan.ranges)
        valid = np.isfinite(ranges) & (ranges > 0.1)

        mask = (np.abs(angles) <= np.radians(30)) & valid
        return float(np.min(ranges[mask])) if np.any(mask) else float('inf')

    def get_side_clearance(self, side: Side):
        """45°~90° 左右清晰度"""
        if not self.scan: return float('inf')
        angles = np.arange(self.scan.angle_min,
                           self.scan.angle_max,
                           self.scan.angle_increment)
        ranges = np.array(self.scan.ranges)
        valid = np.isfinite(ranges)

        if side == Side.LEFT:
            mask = (angles < -np.radians(45)) & (angles > -np.radians(90)) & valid
        else:
            mask = (angles > np.radians(45)) & (angles < np.radians(90)) & valid

        return float(np.mean(ranges[mask])) if np.any(mask) else 0.5

    # ---------------------------------------------------------------------
    # ⭐ Ackermann 控制核心：输入 lateral error → 输出 steering angle（无 S 型）
    # ---------------------------------------------------------------------
    def ackermann_steer(self, lateral_error):
        k_y = 1.2       # 横向增益（已调过，不会摆）
        k_yaw = 2.0     # 航向增益

        desired_yaw = np.arctan2(k_y * lateral_error, self.v)
        yaw_error = desired_yaw - self.yaw

        steer = k_yaw * yaw_error
        steer = np.clip(steer, -self.steer_max, self.steer_max)
        return steer

    def control_loop(self):
        cmd = Twist()

        front = self.get_min_front()
        left = self.get_side_clearance(Side.LEFT)
        right = self.get_side_clearance(Side.RIGHT)

        # ------------------------------------------------------------------
        #                        状 态 机 逻 辑
        # ------------------------------------------------------------------
        if self.state == State.GO_STRAIGHT:
            if front < self.d_detect:
                self.state = State.PREPARE
                self.get_logger().info("‼ 障碍检测：准备规避")

        elif self.state == State.PREPARE:
            if left > right:
                self.target_side = Side.LEFT
                self.get_logger().info("◀ 选择绕左侧")
            else:
                self.target_side = Side.RIGHT
                self.get_logger().info("▶ 选择绕右侧")
            self.state = State.TURN_IN

        elif self.state == State.TURN_IN:
            target_y = self.y_side if self.target_side == Side.LEFT else -self.y_side
            if (self.target_side == Side.LEFT and self.current_y >= 0.8*target_y) or \
               (self.target_side == Side.RIGHT and self.current_y <= 0.8*target_y):
                self.state = State.GO_SIDE
                self.get_logger().info("➡ 进入侧道 GO_SIDE")

        elif self.state == State.GO_SIDE:
            if front > self.d_detect * 1.5:
                self.state = State.TURN_BACK
                self.get_logger().info("↩ 返回中线")

        elif self.state == State.TURN_BACK:
            if abs(self.current_y) < self.y_tol:
                self.state = State.GO_STRAIGHT
                self.get_logger().info("✔ 回到中心线 GO_STRAIGHT")

        # ------------------------------------------------------------------
        #                      Ackermann 控制输出
        # ------------------------------------------------------------------
        if self.state == State.GO_STRAIGHT:
            cmd.linear.x = self.v
            cmd.angular.z = self.ackermann_steer(-self.current_y)

        elif self.state == State.TURN_IN:
            target_y = self.y_side if self.target_side == Side.LEFT else -self.y_side
            lateral_error = target_y - self.current_y
            cmd.linear.x = self.v * 0.7
            cmd.angular.z = self.ackermann_steer(lateral_error)

        elif self.state == State.GO_SIDE:
            target_y = self.y_side if self.target_side == Side.LEFT else -self.y_side
            lateral_error = target_y - self.current_y
            cmd.linear.x = self.v
            cmd.angular.z = self.ackermann_steer(lateral_error)

        elif self.state == State.TURN_BACK:
            lateral_error = -self.current_y
            cmd.linear.x = self.v * 0.7
            cmd.angular.z = self.ackermann_steer(lateral_error)

        self.cmd_pub.publish(cmd)


def main(args=None):
    rclpy.init(args=args)
    node = ObstacleAvoider()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
